#include "mt6701.h"

#define I2C_TIMEOUT_MS      10      // I2C超时时间(ms)

// MT6701 数据缓冲区
uint8_t MT_dat[MT_DAT_LEN] = {0};

/**
 * @brief I2C总线恢复函数 - 当SDA被拉低时通过切换SCL来恢复总线
 */
void MT6701_I2C_Unlock(void)
{
    uint8_t cycleCnt = 0;
    
    // 禁用I2C，将SCL配置为GPIO输出
    DL_I2C_reset(I2C_ENCODER_INST);
    DL_GPIO_initDigitalOutput(GPIO_I2C_ENCODER_IOMUX_SCL);
    DL_GPIO_initDigitalInputFeatures(GPIO_I2C_ENCODER_IOMUX_SDA,
        DL_GPIO_INVERSION_DISABLE, DL_GPIO_RESISTOR_NONE,
        DL_GPIO_HYSTERESIS_DISABLE, DL_GPIO_WAKEUP_DISABLE);
    DL_GPIO_clearPins(GPIO_I2C_ENCODER_SCL_PORT, GPIO_I2C_ENCODER_SCL_PIN);
    DL_GPIO_enableOutput(GPIO_I2C_ENCODER_SCL_PORT, GPIO_I2C_ENCODER_SCL_PIN);
    
    // 通过切换SCL来让从设备释放SDA
    do {
        DL_GPIO_clearPins(GPIO_I2C_ENCODER_SCL_PORT, GPIO_I2C_ENCODER_SCL_PIN);
        delay_ticks(1);
        DL_GPIO_setPins(GPIO_I2C_ENCODER_SCL_PORT, GPIO_I2C_ENCODER_SCL_PIN);
        delay_ticks(1);
        
        // 检查SDA是否被释放
        if (DL_GPIO_readPins(GPIO_I2C_ENCODER_SDA_PORT, GPIO_I2C_ENCODER_SDA_PIN))
            break;
    } while (++cycleCnt < 100);
    
    // 重新启用I2C
    DL_I2C_reset(I2C_ENCODER_INST);
    DL_GPIO_initPeripheralInputFunctionFeatures(GPIO_I2C_ENCODER_IOMUX_SDA,
        GPIO_I2C_ENCODER_IOMUX_SDA_FUNC, DL_GPIO_INVERSION_DISABLE,
        DL_GPIO_RESISTOR_NONE, DL_GPIO_HYSTERESIS_DISABLE,
        DL_GPIO_WAKEUP_DISABLE);
    DL_GPIO_initPeripheralInputFunctionFeatures(GPIO_I2C_ENCODER_IOMUX_SCL,
        GPIO_I2C_ENCODER_IOMUX_SCL_FUNC, DL_GPIO_INVERSION_DISABLE,
        DL_GPIO_RESISTOR_NONE, DL_GPIO_HYSTERESIS_DISABLE,
        DL_GPIO_WAKEUP_DISABLE);
    DL_GPIO_enableHiZ(GPIO_I2C_ENCODER_IOMUX_SDA);
    DL_GPIO_enableHiZ(GPIO_I2C_ENCODER_IOMUX_SCL);
    DL_I2C_enablePower(I2C_ENCODER_INST);
    SYSCFG_DL_I2C_ENCODER_init();
}

/**
 * @brief 从 MT6701 编码器读取数据
 * 
 * @param dev      设备地址 (MT_ADDR)
 * @param reg      寄存器地址 (MT_REG_ANGLE)
 * @param length   要读取的字节数
 * @param data     数据缓冲区指针
 * @return int     0-成功, -1-失败
 */
int MT_Read_Angle(uint8_t dev, uint8_t reg, uint8_t length, uint8_t *data)
{
    uint32_t i = 0;
    uint32_t startTime = msTicks;
    
    if (!length)
        return 0;
    
    // 发送寄存器地址
    DL_I2C_transmitControllerData(I2C_ENCODER_INST, reg);
    
    // 关键：设置自动读取模式 - 发送完寄存器地址后自动切换到读模式
    // 注意：必须先清零MCTR，再启用RD_ON_TXEMPTY
    DL_I2C_resetControllerTransfer(I2C_ENCODER_INST);       // 清零所有控制位
    DL_I2C_enableControllerReadOnTXEmpty(I2C_ENCODER_INST); // 只设置RD_ON_TXEMPTY位
    DL_I2C_clearInterruptStatus(I2C_ENCODER_INST, DL_I2C_INTERRUPT_CONTROLLER_RX_DONE);
    
    // 等待空闲
    while (!(DL_I2C_getControllerStatus(I2C_ENCODER_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
    
    // 启动读取传输
    DL_I2C_startControllerTransfer(I2C_ENCODER_INST, dev, DL_I2C_CONTROLLER_DIRECTION_RX, length);
    
    // 接收数据
    do {
        if (!DL_I2C_isControllerRXFIFOEmpty(I2C_ENCODER_INST)) {
            uint8_t c = DL_I2C_receiveControllerData(I2C_ENCODER_INST);
            if (i < length) {
                data[i] = c;
                ++i;
            }
        }
        
        // 超时检测
        if ((msTicks - startTime) >= I2C_TIMEOUT_MS) {
            MT6701_I2C_Unlock();
            return -1;
        }
    } while (!DL_I2C_getRawInterruptStatus(I2C_ENCODER_INST, DL_I2C_INTERRUPT_CONTROLLER_RX_DONE));
    
    // 读取剩余数据
    if (!DL_I2C_isControllerRXFIFOEmpty(I2C_ENCODER_INST)) {
        uint8_t c = DL_I2C_receiveControllerData(I2C_ENCODER_INST);
        if (i < length) {
            data[i] = c;
            ++i;
        }
    }
    
    // 清除自动读取模式和其他控制位
    DL_I2C_resetControllerTransfer(I2C_ENCODER_INST);
    DL_I2C_flushControllerTXFIFO(I2C_ENCODER_INST);
    
    return (i == length) ? 0 : -1;
}

/**
 * @brief 获取 MT6701 的 14-bit 角度值
 * 
 * @return uint16_t 角度值 (0-16383, 对应 0-360°)
 */
uint16_t MT_Get_Angle(void)
{
    // 读取 2 字节角度数据
    MT_Read_Angle(MT_ADDR, MT_REG_ANGLE, MT_DAT_LEN, MT_dat);
    
    // MT6701 返回 14-bit 角度，高字节在前
    // 数据格式: [高6位][低8位]
    uint16_t angle = ((uint16_t)MT_dat[0] << 8) | MT_dat[1];
    
    // 提取 14 位有效数据（去掉高2位）
    angle = angle & 0x3FFF;
    
    return angle;
}

/**
 * @brief 将角度值转换为度数
 * 
 * @param angle 14-bit 角度值 (0-16383)
 * @return float 角度（度数，0-360°）
 */
float MT_Angle_To_Degree(uint16_t angle)
{
    // 14-bit: 0-16383 对应 0-360°
    return (float)angle * 360.0f / 16384.0f;
}

/**
 * @brief 将角度值转换为弧度
 * 
 * @param angle 14-bit 角度值 (0-16383)
 * @return float 角度（弧度，0-2π）
 */
float MT_Angle_To_Radian(uint16_t angle)
{
    // 14-bit: 0-16383 对应 0-2π
    return (float)angle * 2.0f * PI / 16384.0f;
}

/**
 * @brief 初始化 MT6701 编码器
 * 
 * @return bool true-初始化成功, false-初始化失败
 */
bool MT_Init(void)
{
    // 检查并恢复 I2C 总线
    if (DL_I2C_getSDAStatus(I2C_ENCODER_INST) == DL_I2C_CONTROLLER_SDA_LOW) {
        MT6701_I2C_Unlock();
    }
    // 延时等待 MT6701 上电稳定
    delay_ticks(10);
    // 初始化成功
    return true;
}
